#include "Collision_detector.h"
#include "Rrt_planner.h"
#include "Poly_util.h"
#include "Rrt_Imp.h"

#include <iostream>
#include <streambuf>
#include <fstream>
#include <string>
#include <io.h>

/**
 * read_polygon(int num_vertices, std::istream* in)
 *	read polygon from istream in the specfic format
 */
Polygon	read_polygon(int num_vertices, std::istream* in)
{
	Polygon result;
	for (int i = 0; i < num_vertices; i++)
	{
		double x,y;
		*in >> x >> y;
		Point p(x,y,0);
		result.push_back(p);
	}

	return result;
}
/**
 * load_robot(const char* pi_szFileName)
 *	load robot polygon from pi_szFileName file
 *	return the robot polygon
 */
Polygon	load_robot(const char* pi_szFileName)
{
	std::ifstream ifile(pi_szFileName);
	std::istream *in = &ifile; 

	int num_robots;
	*in >> num_robots;
	
	// IMPORTANT:
	// ---------
	// only the first robot is read while the others are ignored

	int num_vertices;
	*in >> num_vertices;
	Polygon result =  read_polygon(num_vertices, in);
		
	ifile.close();
	return result;
}

/**
 * load_obstacles(const char* pi_szFileName)
 *	load obstacles polygon from pi_szFileName file
 *	return the vector with obstacles polygon
 */
vector<Polygon>	load_obstacles(const char* pi_szFileName)
{
	vector<Polygon> result;
	std::ifstream ifile(pi_szFileName);
	std::istream *in = &ifile; 

	int num_obstacles;
	*in >> num_obstacles;

	if (num_obstacles != 0)
	{
		for (int i = 0; i < num_obstacles; i++)
		{
			int num_vertices;
			*in >> num_vertices;

			Polygon current =  read_polygon(num_vertices, in);
			result.push_back(current);
		}
	}
		
	ifile.close();
	return result;
}

/**
 * load_query(const char* pi_szFileName)
 *	load the query points from pi_szFileName file
 *	return the vector with the sources and targets
 */
vector<Point>	load_query(const char* pi_szFileName)
{

	vector<Point> result;
	std::ifstream ifile(pi_szFileName);
	std::istream *in = &ifile; 
		
	int num_robots;
	*in >> num_robots;

	int num_confs;
	*in >> num_confs;

	for (int i = 0; i < num_confs; i++)
	{		
		double x,y;
		double theta;
		*in >> x >> y >> theta;
		Point point(x,y,theta / 360);
		result.push_back(point);
	}
		
	ifile.close();
	return result;
}
/**
 * save_path(const char* pi_szFileName, list<Point>	path)
 *	save the path found in an robot demand format
 *	save the file to pi_szFileName file. 
 */
void	save_path(const char* pi_szFileName, list<Point>	path)
{
	vector<Point> vec(path.begin(), path.end());
	FILE* fOutput = fopen(pi_szFileName, "w+");
	if (!fOutput)
	{
		return;
	}
	
	if(path.size() == 0)
	{
		fprintf(fOutput, "0\n");
		fclose(fOutput);
		return;
	}	
	int robot_num = 1;
	fprintf(fOutput, "\n%d\n", robot_num);
		
	int size = 2*path.size()-1;
	fprintf(fOutput, "\n%d\n", size);

	fprintf(fOutput, "%.5f %.5f %.5f \n", vec[0][0],vec[0][1],vec[0][2] * 360);

	for (int i = 1; i < vec.size(); i++)
	{
		double x_diff = vec[i][0] - vec[i-1][0];
		double y_diff = vec[i][1] - vec[i-1][1];
		double angle_diff = smallest_angle(vec[i-1],vec[i]);

		fprintf(fOutput, "0.00 0.00 %.5f \n", angle_diff * 360);
		fprintf(fOutput, "%.5f %.5f 0.00 \n", x_diff, y_diff);
	}
	fclose(fOutput);
}
/**
 * save_path_MMS(const char* pi_szFileName, list<Point>	path)
 *	save the path found in an Path optimizer format
 *	save the file to pi_szFileName file. 
 */
void	save_path_MMS(const char* pi_szFileName, list<Point>	path)
{
	vector<Point> vec(path.begin(), path.end());
	FILE* fOutput = fopen(pi_szFileName, "w+");
	if (!fOutput)
	{
		return;
	}
	int size = path.size();

	if (size == 0) 
	{
		fprintf(fOutput, "0\n");
		fclose(fOutput);
		return; 
	}
	int numberOfPoints = 1;

	double curr_angle = 0;
	
	double x = -5 ;
	double y = -5 ;
	double angle = -5;
	double ep = 0.000001;
	for (int i = 0; i < vec.size()-1; i++)
	{
		double angle_diff = smallest_angle(vec[i],vec[i+1]);
		curr_angle += angle_diff*360;
		if(fabs(x-vec[i][0]) > ep || fabs(y-vec[i][1]) > ep || fabs(angle - (vec[i][2] * 360)) > ep)
		{
			fprintf(fOutput, "%.13f %.13f %.13f \n", vec[i][0], vec[i][1], vec[i][2] * 360);
			numberOfPoints++;
			x = vec[i][0];
			y = vec[i][1];
			angle = vec[i][2] * 360;
		}
		if(fabs(x-vec[i][0]) > ep || fabs(y-vec[i][1]) > ep || fabs(angle - curr_angle) > ep)
		{
			fprintf(fOutput, "%.13f %.13f %.13f \n", vec[i][0], vec[i][1],curr_angle);
			numberOfPoints++;
			x = vec[i][0];
			y = vec[i][1];
			angle = curr_angle;
		}
		if(fabs(x-vec[i+1][0]) > ep || fabs(y-vec[i+1][1]) > ep || fabs(angle - curr_angle) > ep)
		{
			fprintf(fOutput, "%.13f %.13f %.13f \n", vec[i+1][0], vec[i+1][1],curr_angle);
			numberOfPoints++;
			x = vec[i+1][0];
			y = vec[i+1][1];
			angle = curr_angle;
		}
	}
	fprintf(fOutput, "%.13f %.13f %.13f \n", vec[vec.size()-1][0], vec[vec.size()-1][1], vec[vec.size()-1][2] * 360);
	fclose(fOutput);

	std::ifstream ifs(pi_szFileName);
	std::string path_points((std::istreambuf_iterator<char>(ifs)),
                 std::istreambuf_iterator<char>());
	
	std::ofstream ofs;
	ofs.open (pi_szFileName);
	ofs << numberOfPoints << endl;
	ofs << path_points;
	ifs.close();
	ofs.close();

}
void	save_path_distance_angle(const char* pi_szFileName, int robot_num,list<Point>	path)
{
	vector<Point> vec(path.begin(), path.end());
	FILE* fOutput = fopen(pi_szFileName, "w+");
	if (!fOutput)
	{
		return;
	}
	double l_diff = 0;
	double angle_diff = 0;

	fprintf(fOutput, "id %d\n", robot_num);
		
	if(path.size() == 0)
	{
		fprintf(fOutput, "0\n");
		fclose(fOutput);
		return;
	}
	fprintf(fOutput, "line %.5f %.5f\n", vec[0][0],vec[0][1]);
	for (int i = 1; i < vec.size(); i++)
	{
		//l_diff = distance_points(vec[i-1],vec[i]);
		l_diff = sqrt(pow((vec[i-1][0]-vec[i][0]),2)+pow((vec[i-1][1]-vec[i][1]),2));
		angle_diff = smallest_angle(vec[i-1],vec[i]);

		if(angle_diff !=0)
		{
			fprintf(fOutput, "angle %.5f\n", angle_diff * 360);
		}
		if(l_diff != 0 )
		{
			fprintf(fOutput, "line %.5f %.5f\n", vec[i][0],vec[i][1]);
		}
	}

	fclose(fOutput);
}
/**
 * check_polygon_simple(const char* p_fileName)
 *	check if the polygon in the file p_fileName
 *	is a simple polygon - ! work only on the first polygon in the file !
 *	return true or false
 */
bool	check_polygon_simple(const char* p_fileName)
{
	Polygon points;
	Polygon_2 poly;
	bool result = false;
	std::ifstream ifile(p_fileName);
	std::istream *in = &ifile; 

	int num_vertices;

	*in >> num_vertices;
	*in >> num_vertices;

	points =  read_polygon(num_vertices, in);
	
	ifile.close();

	for(int i=0;i<points.size();i++)
	{
		poly.push_back(Point_2(points[i][0],points[i][1]));
	}

	return poly.is_simple ()?true:false;
}

bool file_exists(const char * file)
{
	if( access( file, 0 ) == -1 ) 
	{
		return false;
	}
	return true;
}

int main(int argc, char *argv[])
{
	/*
		argv[1] = robot number/path number
		argv[2] = folder path + \
		argv[3] = step size.
		argv[4] = angle step size.
		argv[5] = itration number.
		argv[6] = connect line rate.
		argv[7] = bound of steps

	*/
	// CONSTANTS
	double	step_size = 0.0015; // length of each local_planner step
	double	step_angle_szie = 0.003; // length of each local_planner step
	double	line_step_size = 0.005;
	int		iteration_num = 8000;
	int		connect_line_rate = 20;
	int		bound = 350;
	int fd_name_len = 0;
	int path_num = 0;
	switch(argc)
	{
		case 7:
			connect_line_rate = atoi(argv[6]);
		case 6:
			iteration_num = atoi(argv[5]);
		case 5:
			step_angle_szie = atof(argv[4]);
		case 4:
			step_size = atof(argv[3]);
		case 3:
			fd_name_len = strlen(argv[2]);
		case 2:
			path_num = atoi(argv[1]);
	default:
		break;
	}
	
	string folder = "";
	if(fd_name_len)
	{
		folder = string(argv[2],fd_name_len);
	}
	
	if(!file_exists((folder+"workspace_A.txt").c_str()) || 
	   !file_exists((folder+"robot_S.txt").c_str()) || 
	   !file_exists((folder+"query.txt").c_str()) || argc < 2)
	{
		return 3;
	}
	vector<Polygon>		obstacles = load_obstacles((folder+"workspace_A.txt").c_str());
	Polygon				robot = load_robot((folder+"robot_S.txt").c_str());
	vector<Point>		query = load_query((folder+"query.txt").c_str());

	Polygon				buff_robot = poly_2_to_poly(mink_robot(robot));
	Polygon				mini_robot = dec_size(robot,0.1);

	Collision_detector	col(&obstacles, mini_robot, step_size,step_angle_szie,bound);

	Collision_detector	buff_col(&obstacles, buff_robot, step_size,step_angle_szie,bound);
	
	Rrt_planner planner(&col, &buff_col, query[0], query[1],iteration_num,connect_line_rate,line_step_size,path_num);
	
	list<Point> result = planner.run();
	
	/******/
	Collision_detector	normal_col(&obstacles, robot, step_size,step_angle_szie,50000);
	Rrt_Imp r_imp(&normal_col);
	list<Point> imp_result = r_imp.imp(result);
	save_path((folder+"path_R.txt").c_str(), imp_result);
	save_path_MMS((folder+"path_MMS_new.txt").c_str(),imp_result);
	save_path_distance_angle((folder+"path_robot_imp.txt").c_str(),path_num, imp_result);
	/******/
	save_path_distance_angle((folder+"path_robot.txt").c_str(),path_num, result);
	save_path((folder+"path_try.txt").c_str(), result);
	save_path_MMS((folder+"path"+argv[1]+".txt").c_str(),result);
	
	

	return result.size()>0?1:0;
}